#include <imp_move_filter.h>
#include <imp/imp_log.h>
#ifdef DMALLOC
#include <imp/dmalloc.h>
#endif
#define DEBUG_FLAG 0
#if DEBUG_FLAG
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#endif

#define TAG "MOVEFILTER"

#define MXU_IMAGE_CN(srcType) 1
Image CreateImage(void* src, int height,int width,int type)
{
	Image image;
	image.data=(uint8_t*)src;
	image.type=type;
	image.width=width;
	image.height=height;
	//image.channel=(depth&3)+1;
	image.isContinuous=1;
	image.step=width;
	return image;
}

uint8_t MinuOp(const uint8_t a, const uint8_t b) { return a&b; }
uint8_t MaxuOp(const uint8_t a, const uint8_t b) { return (a>b)?a:b; }
int MinsOp(const int a, const int b) { return (a<b)?a:b; }
int MaxsOp(const int a, const int b) { return (a>b)?a:b; }

int borderInterpolate( int p, int len)
{
	if( (unsigned)p < (unsigned)len )
		;
	else
		p = p < 0 ? 0 : len - 1;
	return p;
}

static const int VEC_ALIGN = 16;
#define ksize filter->ksize
#define anchor filter->anchor
#define maxWidth filter->maxWidth
#define wholeSize filter->wholeSize
#define roi filter->roi
#define dx1 filter->dx1
#define dx2 filter->dx2
#define thresh_val filter->thresh_val
#define max_val filter->max_val
#define maskParamter filter->maskParamter
#define MaskData filter->MaskData
#define Noise_Thresh filter->Noise_Thresh
#define bufStep filter->bufStep
#define startY filter->startY
#define startY0 filter->startY0
#define endY filter->endY
#define rowCount filter->rowCount
#define dstY filter->dstY
#define borderTab filter->borderTab
#define ringBuf filter->ringBuf
#define srcRow filter->srcRow
#define constBorderValue filter->constBorderValue
#define constBorderRow filter->constBorderRow
#define rows filter->rows
#define rowFilter filter->rowFilter
#define columnFilter filter->columnFilter

FilterEngine* Init( zSize Simg, int _cntRoi, IMPRect* roiRect)
{
	FilterEngine* filter = (FilterEngine*)malloc(sizeof(FilterEngine));
	if (NULL == filter) {
		IMP_LOG_ERR(TAG, "malloc error\n");
		return NULL;
	}

	thresh_val = 20;
	max_val = 1;
	ksize.width = 3;
	ksize.height = 3;
	anchor.x = 1;
	anchor.y = 1;
	maxWidth = 0;
	bufStep = 0;
	ringBuf = NULL;
	srcRow = NULL;
	rows = NULL;
	MaskData = NULL;
	maskParamter.Sum = NULL;
	Noise_Thresh = NULL;
	rowFilter = MorphRowFilter;
	columnFilter = MorphColumnFilter;

	assert(0 <= anchor.x && anchor.x < ksize.width &&
			0 <= anchor.y && anchor.y < ksize.height);

	IMPRect rectElem = {roiRect[0].p0, roiRect[0].p1};
	int i;
	for( i = 1; i < _cntRoi; i++){
		rectElem.p0.x = rectElem.p0.x < roiRect[i].p0.x ? rectElem.p0.x : roiRect[i].p0.x;
		rectElem.p0.y = rectElem.p0.y < roiRect[i].p0.y ? rectElem.p0.y : roiRect[i].p0.y;
		rectElem.p1.x = rectElem.p1.x > roiRect[i].p1.x ? rectElem.p1.x : roiRect[i].p1.x;
		rectElem.p1.y = rectElem.p1.y > roiRect[i].p1.y ? rectElem.p1.y : roiRect[i].p1.y;
	}
	zRect srcRoi={rectElem.p0.x, rectElem.p0.y,
		rectElem.p1.x - rectElem.p0.x + 1,
		rectElem.p1.y - rectElem.p0.y + 1};
	roi = srcRoi;

	if (Start(Simg, srcRoi, -1, filter) < 0) {
		IMP_LOG_ERR(TAG, "%s:start failed\n", __func__);
		goto malloc_start;
	}

	Noise_Thresh = (int*)malloc(_cntRoi*sizeof(int));
	if (Noise_Thresh == NULL) {
		IMP_LOG_ERR(TAG, "malloc Noise_Thresh failed\n");
		goto malloc_Noise_Thresh;
	}
	maskParamter.CntRoi = _cntRoi;
	if (updateRoiMask(filter, srcRoi, roiRect) < 0) {
		IMP_LOG_ERR(TAG, "updateRoiMask failed\n");
		goto err_updateRoiMask;

	}
	//printf("----FilterEngine* Init end\n");
	return filter;

err_updateRoiMask:
	free(Noise_Thresh);
malloc_Noise_Thresh:
	free(ringBuf);
	free(srcRow);
	free(rows);
malloc_start:
	free(filter);
	return NULL;
}


int Start(zSize _wholeSize, zRect _roi, int _maxBufRows,FilterEngine* filter)
{
	if(filter==NULL){
		IMP_LOG_ERR(TAG, "null pointer filter\n");
		return -1;
	}
	int i, j;

	wholeSize = _wholeSize;
	roi = _roi;

	if ((roi.x < 0) || (roi.y < 0) || (roi.width < 0) || (roi.height < 0) || (roi.x + roi.width > wholeSize.width) || (roi.y + roi.height > wholeSize.height)) {
		printf("%s(%d):roi.x=%d, roi.y=%d, roi.width=%d, roi.height=%d, wholeSize.width=%d, wholeSize.height=%d\n", __func__, __LINE__, roi.x, roi.y, roi.width, roi.height, wholeSize.width, wholeSize.height);
	}
	assert( roi.x >= 0 && roi.y >= 0 && roi.width >= 0 && roi.height >= 0 &&
			roi.x + roi.width <= wholeSize.width &&roi.y + roi.height <= wholeSize.height );

	int bufElemSize = 1;

	if( _maxBufRows < 0 )
		_maxBufRows = ksize.height + 3;
	_maxBufRows = MaxsOp(_maxBufRows, MaxsOp(anchor.y, ksize.height-anchor.y-1)*2+1);

	//if( maxWidth < roi.width || _maxBufRows != (int)sizeof(rows)/sizeof(uint8_t*) )
	{
		rows = (uint8_t**)malloc(_maxBufRows*sizeof(uint8_t*));
		if (rows == NULL)  {
			IMP_LOG_ERR(TAG, "%s(%d):malloc rows failed\n", __func__, __LINE__);
			goto malloc_rows;
		}
		int rows_size = _maxBufRows;

		maxWidth = MaxsOp(maxWidth, roi.width);
		srcRow = (uint8_t*)malloc(maxWidth + ksize.width - 1);
		if (srcRow == NULL)  {
			IMP_LOG_ERR(TAG, "%s(%d):malloc srcRow failed\n", __func__, __LINE__);
			goto malloc_srcRow;
		}
		int maxBufStep = bufElemSize*(int)alignSize(maxWidth,VEC_ALIGN);
		ringBuf = (uint8_t*)malloc(maxBufStep*rows_size+VEC_ALIGN);
		if (ringBuf == NULL)  {
			IMP_LOG_ERR(TAG, "%s(%d):malloc ringBuf failed\n", __func__, __LINE__);
			goto malloc_ringBuf;
		}
	}

	// adjust bufstep so that the used part of the ring buffer stays compact in memory
	bufStep = bufElemSize*(int)alignSize(roi.width,16);

	dx1 = MaxsOp(anchor.x - roi.x, 0);
	dx2 = MaxsOp(ksize.width - anchor.x - 1 + roi.x + roi.width - wholeSize.width, 0);

	// recompute border tables
	if( dx1 > 0 || dx2 > 0 )
	{
		int xofs1 = MinsOp(roi.x, anchor.x) - roi.x;

		int btab_esz = 1, wholeWidth = wholeSize.width;
		int* btab = (int*)&borderTab[0];

		for( i = 0; i < dx1; i++ )
		{
			int p0 = (borderInterpolate(i-dx1, wholeWidth) + xofs1)*btab_esz;
			for( j = 0; j < btab_esz; j++ )
				btab[i*btab_esz + j] = p0 + j;
		}

		for( i = 0; i < dx2; i++ )
		{
			int p0 = (borderInterpolate(wholeWidth + i, wholeWidth) + xofs1)*btab_esz;
			for( j = 0; j < btab_esz; j++ )
				btab[(i + dx1)*btab_esz + j] = p0 + j;
		}
		//IMP_LOG_ERR(TAG, "%d,%d\n",borderTab[0],borderTab[1]);
	}

	rowCount = dstY = 0;
	startY = startY0 = MaxsOp(roi.y - anchor.y, 0);
	endY = MinsOp(roi.y + roi.height + ksize.height - anchor.y - 1, wholeSize.height);

	//printf("startY = %d, startY0 = %d, endY = %d\n",startY, startY0, endY);
	//printf("maxWidth = %d, roi.width = %d, %d\n",maxWidth, roi.width, _maxBufRows);
	return startY;

malloc_ringBuf:
	free(srcRow);
malloc_srcRow:
	free(rows);
malloc_rows:
	return -1;
}

int remainingInputRows(FilterEngine* filter)
{
	if(filter==NULL){
		IMP_LOG_ERR(TAG, "null pointer filter\n");
		return -1;
	}
	return endY - startY - rowCount;
}

int remainingOutputRows(FilterEngine* filter)
{
	if(filter==NULL){
		IMP_LOG_ERR(TAG, "null pointer filter\n");
		return -1;
	}
	return roi.height - dstY;
}

#if DEBUG_FLAG
int file_src = 0;
int kcount = 0;
#endif
int Proceed( const uint8_t* src1, int src1step,const uint8_t* src2, int src2step,
		uint8_t* dst, int dststep,FilterEngine* filter, int isUseSimd)
{
#if DEBUG_FLAG
	kcount++;
	int sum_k = 0;
#endif
	assert( wholeSize.width > 0 && wholeSize.height > 0 );
	const int *btab = &borderTab[0];
	int esz = 1;
	uint8_t** brows = &rows[0];
	//int btab_esz = 1;
	int bufRows = 6;
	int width = roi.width, kwidth = ksize.width;
	int kheight = ksize.height, ay = anchor.y;
	int _dx1 = dx1;
	int _dx2 = dx2;
	int width1 = roi.width + kwidth - 1;
	int xofs1 = MinsOp(roi.x, anchor.x);
	int dy = 0, i = 0;

	int count = endY - startY;
	src1 -= xofs1*esz;
	src2 -= xofs1*esz;
	//printf("xofs1*esz = %d, dx = %d,dx2 = %d, width1 = %d\n",xofs1*esz,_dx1,_dx2,width1);
	count = MinsOp(count, remainingInputRows(filter));

	//printf("btab[0] = %d,btab[1] = %d\n",btab[0],btab[1]);
	assert( src1 && src2 && dst && filter && count > 0 );

#if 0
	v16u8 v0,v1, vec_th, vec_one;
#endif
	if (isUseSimd) {
        assert(0);
#if 0
		vec_th = _mx128_mfcpu_b(thresh_val);
		vec_one = _mx128_li_b(1);
#endif
	}
	for (;; dst += dststep*i, dy += i) {
		int dcount = bufRows - ay - startY - rowCount + roi.y;
		dcount = dcount > 0 ? dcount : bufRows - kheight + 1;
		dcount = MinsOp(dcount, count);
		count -= dcount;
		for( ; dcount-- > 0; src1 += src1step,src2 += src2step )
		{
			int bi = (startY - startY0 + rowCount) % bufRows;
			uint8_t* brow = alignPtr(&ringBuf[0], VEC_ALIGN) + bi*bufStep;
			uint8_t* row = &srcRow[0];
			uint8_t* row_addr = row + _dx1*esz;
			int swidth = (width1 - _dx2 - _dx1)*esz;
			if( ++rowCount > bufRows )
			{
				--rowCount;
				++startY;
			}
			i = 0;
			if (isUseSimd) {
        assert(0);
#if 0
				for(i=0;i<=swidth-16;i+=16){
					LU1Q(v0,src1+i,0);
					LU1Q(v1,src2+i,0);
					SUBUA(b,v0,v0,v1);
					CLTU(b,v0,vec_th,v0);
					ANDV(v0,v0,vec_one);
					SU1Q(v0,row_addr+i,0);
				}
#endif
			} else {
				for(i=0; i<=swidth-4; i+=4){
					row_addr[i] = (abs(src1[i]-src2[i])> thresh_val);
					row_addr[i+1] = (abs(src1[i+1]-src2[i+1])> thresh_val);
					row_addr[i+2] = (abs(src1[i+2]-src2[i+2])> thresh_val);
					row_addr[i+3] = (abs(src1[i+3]-src2[i+3])> thresh_val);
				}
			}
			for(;i<swidth;i++)
				row_addr[i] = (abs(src1[i]-src2[i])> thresh_val) /* ? max_val:0 */;

#if 1
			for( i = 0; i < _dx1*esz; i++ )
				row[i] = abs(src1[btab[i]] - src2[btab[i]]) > thresh_val;
			for( i = 0; i < _dx2*esz; i++ )
				row[i + (width1 - _dx2)*esz] = abs(src1[btab[i+_dx1*esz]] - src2[btab[i+_dx1*esz]]) > thresh_val;

#else

			row[0] = row_addr[0];
			row[width1 - 1] = row_addr[width-1];
#endif

#if DEBUG_FLAG
			int k = 0, t = 0;

			uint8_t dsrc[width];
			for(t = 0;t<width;t++){
				//dsrc[t] = (abs(src1[t]-src2[t])> thresh_val) ? 255:0;
				//dsrc[t] = src1[t];
				dsrc[t] = (row_addr[t+1] >0 )? 255:0;
				sum_k += (row_addr[t]>0);
			}

			if(file_src == 0)
				file_src = open("src_raw.data",O_RDWR|O_CREAT|O_APPEND);
			write(file_src,dsrc,width);
#endif
			rowFilter(row, brow, width, MXU_IMAGE_CN(srcType),ksize.width,isUseSimd);

		}

		int max_i = MinsOp(bufRows, roi.height - (dstY + dy) + (kheight - 1));
		for( i = 0; i < max_i; i++ )
		{
			int srcY = borderInterpolate(dstY + dy + i + roi.y - ay,wholeSize.height);
			{

				assert( srcY >= startY );
				if( srcY >= startY + rowCount )
					break;
				int bi = (srcY - startY0) % bufRows;
				brows[i] = alignPtr(&ringBuf[0], VEC_ALIGN) + bi*bufStep;
			}
		}
		if( i < kheight )
			break;
		i -= kheight - 1;

		columnFilter((const uint8_t**)brows, dst, dststep, i, roi.width, ksize.height, &maskParamter, isUseSimd);
	}

#if DEBUG_FLAG
	printf("sum_k = %d\n",sum_k);
	if(kcount==100){
		close(file_src);
		printf("close file\n");
		return 0;
	}
#endif
	dstY += dy;
	assert( dstY <= roi.height );
	return dy;
}

void freeFilterEngine(FilterEngine* filter)
{
	if(filter==NULL)
		return;

	if(MaskData != NULL){
		free(MaskData);
		MaskData = NULL;
	}
	if(Noise_Thresh!= NULL){
		free(Noise_Thresh);
		Noise_Thresh = NULL;
	}
	if(rows){
		free(rows);
		rows = NULL;
	}
	if(srcRow){
		free(srcRow);
		srcRow = NULL;
	}
	if(ringBuf){
		free(ringBuf);
		ringBuf = NULL;
	}

	free(filter);
	filter=NULL;
}

//define TEST_MASK
int updateRoiMask(FilterEngine* filter, zRect srcRoi, IMPRect* _roi)
{
	int i = 0;
	int j = 0;
	if( maskParamter.CntRoi <= 0 ){
		IMP_LOG_ERR("Outside Error", "Bad paramter of roi!! Please set it bigger than 0!\n");
		return -1;
	}

	if( NULL != MaskData ){
		free( MaskData );
	}

	int _width = srcRoi.width;
	int _height = srcRoi.height;
	int block = ( maskParamter.CntRoi/8 + ((maskParamter.CntRoi%8)!=0) );
	int RowMaskSize = _height*block*sizeof(int8_t);
	int ColumnMaskSize = _width*block*sizeof(int8_t);
	maskParamter.maskSize = ColumnMaskSize + RowMaskSize + maskParamter.CntRoi*sizeof(int);
	MaskData = (int8_t*)malloc(maskParamter.maskSize);
	if(MaskData == NULL){
		IMP_LOG_ERR(TAG, "%s(%d):malloc MaskData error\n", __func__, __LINE__);
		return -1;
	}

	memset(MaskData, 0, maskParamter.maskSize);
	maskParamter.Rowmask = (int8_t*)(&MaskData[0]);
	maskParamter.Columnmask = (int8_t*)(&MaskData[RowMaskSize]);
	maskParamter.Sum = (int*)(&MaskData[RowMaskSize + ColumnMaskSize]);
	maskParamter.Cstep = _width;
	maskParamter.Rstep = _height;
	maskParamter.County = 0;

	//printf("roi: %d, %d, %d, %d\n",srcRoi.x,srcRoi.y,srcRoi.width,srcRoi.height);
	int location;
	for(j = 0; j < maskParamter.CntRoi; j++){
		IMPRect Roi = _roi[j];
		for(i = 0; i < _width ; i++){
			location = i + srcRoi.x;
			if(Roi.p0.x <= location &&  Roi.p1.x >= location){
				maskParamter.Columnmask[i+maskParamter.Cstep*(j>>3)] |= (1<<(j%8));
				//printf("column:%d-save in[%d]-----(%d,%d)---\n",i,i+maskParamter.Cstep*(j>>3),j,j%8);
			}
		}

		for(i = 0; i < _height ; i++){
			location = i + srcRoi.y;
			if(Roi.p0.y <= location &&  Roi.p1.y >= location){
				maskParamter.Rowmask[i+maskParamter.Rstep*(j>>3)] |= (1<<(j%8));
				//maskParamter.Rowmask[i] |= (1<<j);
				//printf("row:%d------(%d,%d)---\n",i,j);
			}
		}

	}

#ifdef TEST_MASK
	for(i = 0; i < maskParamter.maskSize; i++){
		printf("%d",MaskData[i]);
	}
	printf("\n");
#endif

	return 0;
}
